#include "plugin.h"
#include <math.h>

CameraLibrary::Camera *gCamera=0;
CameraLibrary::Frame *gFrame=0;
CameraLibrary::cModuleVector *vec = CameraLibrary::cModuleVector::Create(); //new cModuleVector();
CameraLibrary::cModuleVectorProcessing *vecprocessor = new CameraLibrary::cModuleVectorProcessing();
Core::DistortionModel lensDistortion;

int bufferSize = 10;

void wSetupCamera() {
	if(!wAreCamerasInitialized()) {
		CameraLibrary::CameraManager::X().WaitForInitialization();
	}
}

bool wAreCamerasInitialized() {
	return CameraLibrary::CameraManager::X().AreCamerasInitialized();
}

void wShutdownCamera() {
	if(wAreCamerasInitialized()) {
		CameraLibrary::CameraManager::X().Shutdown();
	}
}

bool wStartCamera() {
	gCamera = CameraLibrary::CameraManager::X().GetCamera();
	if(gCamera) {
		gCamera->SetVideoType(CameraLibrary::SegmentMode);
		//gCamera->SetTextOverlay(false);
		//gCamera->SetIntensity(5);
		//gCamera->SetExposure(3);
		//gCamera->SetThreshold(200);
		gCamera->GetDistortionModel(lensDistortion);
		gCamera->Start();

		setCameraData();

		return true;
	} else {
		return false;
	}
}

bool wHaveCamera() {
	if(gCamera) {
		return true;
	} else {
		return false;
	}
}

bool wStopCamera() {
	if(gCamera) {
		gCamera->Stop();
		gCamera->Release();
		gCamera = 0;
	}
	return true;
}

float* wGetMatrix(void) {
	static float* matrixData = new float[16];
	matrixData = ResetIdentity();
	matrixData[0] = 123;
	
	if(wAreCamerasInitialized() == false && wHaveCamera() == false) {
		wSetupCamera();
		wStartCamera();
		matrixData[0] = 987;
	} else if(wAreCamerasInitialized() == true && wHaveCamera() == false) {
		wStopCamera();
		wStartCamera();
		matrixData[0] = 654;

	} else {
		matrixData[0] = 456;
		static double yawBuffer[10];
		static double pitchBuffer[10];
		static double rollBuffer[10];

		CameraLibrary::Frame *frame = gCamera->GetLatestFrame();
		if(frame!=0) {
			matrixData[0] = 789;

			if(frame->ObjectCount() >= 3) {
				float x = 0;
				float y = 0;

				vec->BeginFrame();
				for(int i=0; i<frame->ObjectCount(); i++) {
					CameraLibrary::cObject *obj = frame->Object(i);
					x = obj->X();
					y = obj->Y();
					Core::Predistort2DPoint(lensDistortion,x,y);
					vec->PushMarkerData(x, y, obj->Area(), obj->Width(), obj->Height());
				}
				vec->Calculate();
				vecprocessor->PushData(vec);

				if(vecprocessor->MarkerCount()>0) {
					double x,y,z;
					double yaw,pitch,roll;
					vecprocessor->GetPosition(x,y,z);
					vecprocessor->GetOrientation(yaw,pitch,roll);

					if(yaw == 0.0 && pitch == 0.0 && roll == 0.0) {
					} else {
						updateBuffer(yawBuffer, yaw, pitchBuffer, pitch, rollBuffer, roll);
					}
					smoothed(yawBuffer, &yaw, pitchBuffer, &pitch, rollBuffer, &roll);

					float quat[]  = { 0.0f, 0.0f, 0.0f, 1.0f };
					float qYaw[]   = { 0.0f, 0.0f, 0.0f, 1.0f };
					float qPitch[] = { 0.0f, 0.0f, 0.0f, 1.0f };
					float qRoll[]  = { 0.0f, 0.0f, 0.0f, 1.0f };
					int oYaw[] = { 0, 1, 0 };
					int oPitch[] = { 1, 0, 0 };
					int oRoll[] = { 0, 0, 1 };

					quatCreateFromAxisAngle(qYaw, oYaw, degToRad((float)yaw));
					quatCreateFromAxisAngle(qPitch, oPitch, degToRad((float)pitch));
					quatCreateFromAxisAngle(qRoll, oRoll, degToRad((float)roll));

					quatMultiply(quat, qYaw);
					quatMultiply(quat, qPitch);
					quatMultiply(quat, qRoll);

					quatToMat4(matrixData, quat);

					matrixData[12] = (float)x;
					matrixData[13] = (float)y;
					matrixData[14] = (float)z;
				}

			}
			frame->Release();
		}
	}
	return matrixData;
}

void wRecenter(void) {
	vecprocessor->Recenter();
}

void setCameraData() {
	vec = CameraLibrary::cModuleVector::Create(); //new cModuleVector();
	vecprocessor = new CameraLibrary::cModuleVectorProcessing();

	CameraLibrary::cVectorSettings vectorSettings;
    vectorSettings = *vec->Settings();
    vectorSettings.Arrangement = CameraLibrary::cVectorSettings::VectorClip;
	//vectorSettings.Arrangement = CameraLibrary::cVectorSettings::TrackClipPro;
    vectorSettings.Enabled     = true;

	CameraLibrary::cVectorProcessingSettings vectorProcessorSettings;
    vectorProcessorSettings = *vecprocessor->Settings();
    vectorProcessorSettings.Arrangement = CameraLibrary::cVectorSettings::VectorClip;
	//vectorProcessorSettings.Arrangement = CameraLibrary::cVectorSettings::TrackClipPro;
    vectorProcessorSettings.ShowPivotPoint = false;
    vectorProcessorSettings.ShowProcessed  = false;
    vecprocessor->SetSettings(vectorProcessorSettings);

	vectorSettings.ImagerFocalLength =  (lensDistortion.HorizontalFocalLength/((float) gCamera->PhysicalPixelWidth()))*gCamera->ImagerWidth();
    vectorSettings.ImagerHeight = gCamera->ImagerHeight();
    vectorSettings.ImagerWidth  = gCamera->ImagerWidth();
    vectorSettings.PrincipalX   = gCamera->PhysicalPixelWidth()/2;
    vectorSettings.PrincipalY   = gCamera->PhysicalPixelHeight()/2;
    vectorSettings.PixelWidth   = gCamera->PhysicalPixelWidth();
    vectorSettings.PixelHeight  = gCamera->PhysicalPixelHeight();
    vec->SetSettings(vectorSettings);
}

float* ResetIdentity() {
	float* identity = new float[16];
	identity[0]  = 1.0f;  identity[1] = 0.0f;  identity[2] = 0.0f;  identity[3] = 0.0f;
	identity[4]  = 0.0f;  identity[5] = 1.0f;  identity[6] = 0.0f;  identity[7] = 0.0f;
	identity[8]  = 0.0f;  identity[9] = 0.0f; identity[10] = 1.0f; identity[11] = 0.0f;
	identity[12] = 0.0f; identity[13] = 0.0f; identity[14] = 0.0f; identity[15] = 1.0f;
	return identity;
}

float degToRad(float degree) {
	return degree * 3.142f / 180;
}

void quatCreateFromAxisAngle(float* quat, int order[], float degree) {
    float r = sin(degree / 2);
    quat[0] = order[0] * r;
    quat[1] = order[1] * r;
    quat[2] = order[2] * r;
    quat[3] = cos(degree / 2);
}

void quatMultiply(float* quat1, float quat2[]) {
	float d,e,g,a,f,h,i,b;
	d = quat1[0];
	e = quat1[1];
	g = quat1[2];
	a = quat1[3];
	f = quat2[0];
	h = quat2[1];
	i = quat2[2];
	b = quat2[3];

	quat1[0] = d*b+a*f+e*i-g*h;
    quat1[1] = e*b+a*h+g*f-d*i;
    quat1[2] = g*b+a*i+d*h-e*f;
    quat1[3] = a*b-d*f-e*h-g*i;
}

void quatToMat4(float* matrixData, float quat[]) {
	//matrixData = ResetIdentity();
	float c, d, e, g, f, h, i, j, k, l;
	c = quat[0];
	d = quat[1];
	e = quat[2];
	g = quat[3];

	f = c+c;
	h = d+d;
	i = e+e;
	j = c*f;
	k = c*h;
	c = c*i;
	l = d*h;
	d = d*i;
	e = e*i;
	f = g*f;
	h = g*h;
	g = g*i;

	matrixData[0]  = 1-(l+e);
	matrixData[1]  = k-g;
	matrixData[2]  = c+h;
	matrixData[3]  = 0;
	matrixData[4]  = k+g;
	matrixData[5]  = 1-(j+e);
	matrixData[6]  = d-f;
	matrixData[7]  = 0;
	matrixData[8]  = c-h;
	matrixData[9]  = d+f;
	matrixData[10] = 1-(j+l);
	matrixData[11] = 0;
	matrixData[12] = 0;
	matrixData[13] = 0;
	matrixData[14] = 0;
	matrixData[15] = 1;
}

void updateBuffer(double* yawBuffer, double yaw, double* pitchBuffer, double pitch, double* rollBuffer, double roll) {
	for(int i = 0; i < (bufferSize-1); i++) { 
		yawBuffer[i] = yawBuffer[i+1];
		pitchBuffer[i] = pitchBuffer[i+1];
		rollBuffer[i] = rollBuffer[i+1];
	}
	yawBuffer[(bufferSize-1)] = yaw;
	pitchBuffer[(bufferSize-1)] = pitch;
	rollBuffer[(bufferSize-1)] = roll;
}

void smoothed(double* yawBuffer, double* yaw, double* pitchBuffer, double* pitch, double* rollBuffer, double* roll) {
	double totalYaw = 0, totalPitch = 0, totalRoll = 0;
	for(int i = 0; i < bufferSize; i++) { 
		totalYaw += yawBuffer[i];
		totalPitch += pitchBuffer[i];
		totalRoll += rollBuffer[i];
	}
	*yaw = totalYaw / bufferSize;
	*pitch = totalPitch / bufferSize;
	*roll = totalRoll / bufferSize;
}
